#include "inf_ntc.h"

#include "drv_gpio.h"
#include "common_debug.h"

#define NTC_EN_PI_NUM GET_PIN_NUM(D, 3) /* NTC使能引脚 */

#define PID0_KPx1000 (int32_t)2600
#define PID0_KIx1000 (int32_t)5
#define PID0_KDx1000 (int32_t)40000

#define PID1_KPx1000 (int32_t)2600
#define PID1_KIx1000 (int32_t)5
#define PID1_KDx1000 (int32_t)40000

#define PID2_KPx1000 (int32_t)2600
#define PID2_KIx1000 (int32_t)5
#define PID2_KDx1000 (int32_t)40000

#define PID_ERR_INT_MAX (int32_t)3000
#define PID_ERR_INT_MIN (int32_t)-3000

#define NTC_HEAT_GEAR_LOW  33U
#define NTC_HEAT_GEAR_MID  66U
#define NTC_HEAT_GEAR_HIGH 100U

// 加热档位 温度设置
#define HEATER_GEARS_DEG_X10 (int16_t[]){ 250, 300, 450 }

/**
 * @brief NTC模块-PID控制结构体
 *
 */
typedef struct
{
    int32_t kp;       /*!< PID比例系数 */
    int32_t ki;       /*!< PID积分系数 */
    int32_t kd;       /*!< PID微分系数 */
    int32_t err;      /*!< 当前误差 */
    int32_t err_last; /*!< 上一次误差 */
    int32_t err_int;  /*!< 积分误差 */
} pid_t;

static result_e inf_ntc_en_pin_init(void);
static void     inf_ntc_pid_init(void);
static int16_t  inf_ntc_get_temp(uint16_t adc_value);

static pid_t ntc_pid;

/*
    31个数据，对应ADC的值从 1*128 = 128， 到31*128 = 3968；
    温度为degC * 10
*/
// B25 = 3435, R25 = 33kohm
static const int16_t ntc_data[31] = { 1181, 882,  722,  614,  531,  464,  408,  359,  315, 275, 238,
                                      203,  171,  140,  110,  81,   52,   24,   -4,   -32, -61, -90,
                                      -120, -151, -185, -221, -261, -306, -360, -430, -537 };

/**
 * @brief 初始化NTC模块
 *
 * @return result_e 初始化结果, RESULT_STATUS_OK表示成功, 其他值表示失败
 */
result_e inf_ntc_init(void)
{
    /* 初始化NTC使能引脚 */
    CHECK_RESULT(inf_ntc_en_pin_init(), "NTC en pin init fail\n");

    /* 使能NTC模块 */
    CHECK_RESULT(inf_ntc_enable(true), "NTC enable fail\n");

    /* 初始化NTC模块PID控制器 */
    inf_ntc_pid_init();

    return RESULT_STATUS_OK;
}

/**
 * @brief 获取NTC模块PID控制器输出（PWM占空比）
 *
 * @return int16_t PID控制器输出（PWM占空比）
 */
int16_t inf_ntc_get_pid_output(uint32_t heat_gear, uint16_t adc_value)
{
    // 获取PID系数
    switch (heat_gear) {
    case NTC_HEAT_GEAR_LOW:
        ntc_pid.kp = PID0_KPx1000;
        ntc_pid.ki = PID0_KIx1000;
        ntc_pid.kd = PID0_KDx1000;
        break;
    case NTC_HEAT_GEAR_MID:
        ntc_pid.kp = PID1_KPx1000;
        ntc_pid.ki = PID1_KIx1000;
        ntc_pid.kd = PID1_KDx1000;
        break;
    case NTC_HEAT_GEAR_HIGH:
        ntc_pid.kp = PID2_KPx1000;
        ntc_pid.ki = PID2_KIx1000;
        ntc_pid.kd = PID2_KDx1000;
        break;
    default:
        ntc_pid.kp = PID0_KPx1000;
        ntc_pid.ki = 0;
        ntc_pid.kd = 0;
        break;
    }
    // 暂存历史误差
    ntc_pid.err_last = ntc_pid.err;
    // 更新当前误差
    ntc_pid.err      = -inf_ntc_get_temp(adc_value + HEATER_GEARS_DEG_X10[heat_gear]);
    // 更新误差积分
    ntc_pid.err_int  = ntc_pid.err_int + ntc_pid.ki * ntc_pid.err;
    if (ntc_pid.err_int > PID_ERR_INT_MAX) ntc_pid.err_int = PID_ERR_INT_MAX;
    if (ntc_pid.err_int < PID_ERR_INT_MIN) ntc_pid.err_int = PID_ERR_INT_MIN;
    // 计算积分项
    int32_t pid_int  = ntc_pid.err_int * ntc_pid.ki;
    // 计算微分项
    int32_t pid_diff = ntc_pid.kd * (ntc_pid.err - ntc_pid.err_last);
    // 计算比例项
    int32_t pid_prop = ntc_pid.kp * ntc_pid.err;
    // 计算PWM
    int32_t pid_out  = (pid_prop + pid_int + pid_diff) >> 10; // 除以1000
    if (pid_out < 0) {
        return 0;
    }
    else if (pid_out > 100) {
        return 100;
    }
    else {
        return (int16_t)pid_out;
    }
}

/**
 * @brief 使能NTC模块
 *
 * @param  enable   是否使能NTC模块， true表示使能, false表示禁用
 * @return result_e 使能结果, RESULT_STATUS_OK表示成功, 其他值表示失败
 */
result_e inf_ntc_enable(bool enable)
{
    if (enable) {
        CHECK_RESULT(snf_drv_gpio_write_pin(NTC_EN_PI_NUM, DRV_GPIO_PIN_LEVEL_HIGH),
                     "NTC en pin set high fail\n");
    }
    else {
        CHECK_RESULT(snf_drv_gpio_write_pin(NTC_EN_PI_NUM, DRV_GPIO_PIN_LEVEL_LOW),
                     "NTC en pin set low fail\n");
    }

    return RESULT_STATUS_OK;
}

/**
 * @brief 获取NTC模块温度（degC * 10）
 *
 * @param  adc_value  NTC模块ADC值
 * @return int16_t    温度（degC * 10）
 */
static int16_t inf_ntc_get_temp(uint16_t adc_value)
{
    // interpolate temperature from ntc_data[31]
    // 超过NTC数据的上下限
    if (adc_value <= 128) return ntc_data[0];
    if (adc_value >= 3968) return ntc_data[30];
    // 正常数据
    uint16_t tmp = adc_value >> 7;
    int16_t  x0  = (int32_t)(tmp << 7);
    int16_t  y0  = ntc_data[tmp];
    int16_t  y1  = ntc_data[tmp + 1];

    return (int16_t)(y0 + ((y1 - y0) * (adc_value - x0) / 128));
}

/**
 * @brief 初始化NTC模块PID控制器
 *
 */
static void inf_ntc_pid_init(void)
{
    ntc_pid.err      = 0;
    ntc_pid.err_last = 0;
    ntc_pid.kp       = PID0_KPx1000;
    ntc_pid.ki       = PID0_KIx1000;
    ntc_pid.kd       = PID0_KDx1000;
    ntc_pid.err      = 0;
    ntc_pid.err_last = 0;
    ntc_pid.err_int  = 0;
}

/**
 * @brief 初始化NTC使能引脚
 *
 * @return result_e 初始化结果, RESULT_STATUS_OK表示成功, 其他值表示失败
 */
static result_e inf_ntc_en_pin_init(void)
{
    snf_gpio_config_t gpio_config = {
        .pin_num    = NTC_EN_PI_NUM,
        .mode       = DRV_GPIO_PIN_MODE_OUTPUT,
        .init_level = DRV_GPIO_PIN_LEVEL_LOW,
    };
    CHECK_RESULT(snf_drv_gpio_init(&gpio_config), "NTC en pin init fail\n");

    return RESULT_STATUS_OK;
}
